/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"	// Precompiled headers
//
#include <mrpt/core/bits_math.h>
#include <mrpt/hwdrivers/CPtuDPerception.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>

#include <cstring>
#include <iostream>
#include <thread>

using namespace std;
using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::system;

/*-------------------------------------------------------------
						moveToAbsPos
-------------------------------------------------------------*/

bool CPtuDPerception::moveToAbsPos(char axis, double nRad)
{
	if (!radAsign(axis, 'P', nRad)) return false;

	return true;
}

/*-------------------------------------------------------------
						absPosQ
-------------------------------------------------------------*/

bool CPtuDPerception::absPosQ(char axis, double& nRad)
{
	return radQuerry(axis, 'P', nRad);
}

/*-------------------------------------------------------------
						moveToOffPos
-------------------------------------------------------------*/

bool CPtuDPerception::moveToOffPos(char axis, double nRad)
{
	if (!radAsign(axis, 'O', nRad)) return false;

	return true;
}

/*-------------------------------------------------------------
						offPosQ
-------------------------------------------------------------*/

bool CPtuDPerception::offPosQ(char axis, double& nRad)
{
	return radQuerry(axis, 'O', nRad);
}

/*-------------------------------------------------------------
						maxPosQ
-------------------------------------------------------------*/

bool CPtuDPerception::maxPosQ(char axis, double& nRad)
{
	return radQuerry(axis, 'X', nRad);
}

/*-------------------------------------------------------------
						minPosQ
-------------------------------------------------------------*/

bool CPtuDPerception::minPosQ(char axis, double& nRad)
{
	return radQuerry(axis, 'N', nRad);
}

/*-------------------------------------------------------------
						speed
-------------------------------------------------------------*/

bool CPtuDPerception::speed(char axis, double radSec)
{
	return radAsign(axis, 'S', radSec);
}

/*-------------------------------------------------------------
						speedQ
-------------------------------------------------------------*/

bool CPtuDPerception::speedQ(char axis, double& radSec)
{
	return radQuerry(axis, 'S', radSec);
}

/*-------------------------------------------------------------
						aceleration
-------------------------------------------------------------*/

bool CPtuDPerception::aceleration(char axis, double radSec2)
{
	return radAsign(axis, 'A', radSec2);
}

/*-------------------------------------------------------------
						acelerationQ
-------------------------------------------------------------*/

bool CPtuDPerception::acelerationQ(char axis, double& radSec2)
{
	return radQuerry(axis, 'A', radSec2);
}

/*-------------------------------------------------------------
						baseSpeed
-------------------------------------------------------------*/

bool CPtuDPerception::baseSpeed(char axis, double radSec)
{
	return radAsign(axis, 'B', radSec);
}

/*-------------------------------------------------------------
						baseSpeedQ
-------------------------------------------------------------*/

bool CPtuDPerception::baseSpeedQ(char axis, double& radSec)
{
	return radQuerry(axis, 'B', radSec);
}

/*-------------------------------------------------------------
						upperSpeed
-------------------------------------------------------------*/

bool CPtuDPerception::upperSpeed(char axis, double radSec)
{
	return radAsign(axis, 'U', radSec);
}

/*-------------------------------------------------------------
						upperSpeedQ
-------------------------------------------------------------*/

bool CPtuDPerception::upperSpeedQ(char axis, double& radSec)
{
	return radQuerry(axis, 'U', radSec);
}

/*-------------------------------------------------------------
						lowerSpeed
-------------------------------------------------------------*/

bool CPtuDPerception::lowerSpeed(char axis, double radSec)
{
	return radAsign(axis, 'L', radSec);
}

/*-------------------------------------------------------------
						lowerSpeedQ
-------------------------------------------------------------*/

bool CPtuDPerception::lowerSpeedQ(char axis, double& radSec)
{
	return radQuerry(axis, 'L', radSec);
}

/*-------------------------------------------------------------
						enableLimitsQ
-------------------------------------------------------------*/

bool CPtuDPerception::enableLimitsQ(bool& enable)
{
	char response[150];

	if (!transmit("L") || !receive("L", response)) return false;

	if (strstr(upperCase(response).c_str(), "ENABLE") != nullptr) enable = true;
	else
		enable = false;

	return true;
}

/*-------------------------------------------------------------
						enableLimits
-------------------------------------------------------------*/

bool CPtuDPerception::enableLimits(bool set)
{
	if (set) return (transmit("LE") && receive("LE", nullptr));
	else
		return (transmit("LD") && receive("LD", nullptr));
}

/*-------------------------------------------------------------
					inmediateExecution
-------------------------------------------------------------*/

bool CPtuDPerception::inmediateExecution(bool set)
{
	if (set) return (transmit("I") && receive("I", nullptr));
	else
		return (transmit("S") && receive("S", nullptr));
}

/*-------------------------------------------------------------
						aWait
-------------------------------------------------------------*/

bool CPtuDPerception::aWait()
{
	return (transmit("A") && receive("A", nullptr));
}

/*-------------------------------------------------------------
						haltAll
-------------------------------------------------------------*/

bool CPtuDPerception::haltAll()
{
	return (transmit("H") && receive("H", nullptr));
}

/*-------------------------------------------------------------
						halt
-------------------------------------------------------------*/

bool CPtuDPerception::halt(char axis)
{
	char sTrans[3];
	sTrans[0] = 'H';
	sTrans[1] = axis;
	sTrans[2] = '\0';

	return (transmit(sTrans) && receive(sTrans, nullptr));
}

/*-------------------------------------------------------------
						reset
-------------------------------------------------------------*/

bool CPtuDPerception::reset()
{
	if (!transmit("R")) return false;
	receive("R", nullptr);

	return panTiltHitError();
}

/*-------------------------------------------------------------
						save
-------------------------------------------------------------*/

bool CPtuDPerception::save()
{
	return (transmit("DS") && receive("DS", nullptr));
}

/*-------------------------------------------------------------
					restoreDefaults
-------------------------------------------------------------*/

bool CPtuDPerception::restoreDefaults()
{
	return (transmit("DR") && receive("DR", nullptr));
}

/*-------------------------------------------------------------
				restoreFactoryDefaults
-------------------------------------------------------------*/

bool CPtuDPerception::restoreFactoryDefaults()
{
	return (transmit("DF") && receive("DF", nullptr));
}

/*-------------------------------------------------------------
						version
-------------------------------------------------------------*/

bool CPtuDPerception::version(char* sVersion)
{
	return (transmit("V") && receive("V", sVersion));
}

/*-------------------------------------------------------------
						powerModeQ
-------------------------------------------------------------*/

bool CPtuDPerception::powerModeQ(bool transit, char& mode)
{
	const char* response = "";

	if (transit)
	{
		if (!transmit("PM")) return false;
		else if (!transmit("PH"))
			return false;
	}

	if (strstr(upperCase(response).c_str(), "REGULAR") != nullptr)
		mode = Regular;
	else if (strstr(upperCase(response).c_str(), "HIGH") != nullptr)
		mode = High;
	else if (strstr(upperCase(response).c_str(), "LOW") != nullptr)
		mode = Low;
	else  // OFF
		mode = Off;

	return true;
}

/*-------------------------------------------------------------
						powerMode
-------------------------------------------------------------*/

bool CPtuDPerception::powerMode(bool transit, char mode)
{
	char sTrans[4];	 //="";
	sTrans[0] = 'P';
	sTrans[1] = transit ? 'M' : 'H';
	sTrans[2] = mode;
	sTrans[3] = '\0';

	return (transmit(sTrans) && receive(sTrans, nullptr));
}

/*-------------------------------------------------------------
							init
-------------------------------------------------------------*/

bool CPtuDPerception::init(const string& port)
{
	try
	{
		serPort.open(port);

		cout << endl << "[INFO] Start PTU communication config:" << endl;

		cout << "[PTU::OpenSerialPort] Opening serial port...";

		if (serPort.isOpen()) {}
		else
		{
			cout << " Error opening serial port";
			return false;
		}

		cout << "OK" << endl;

		cout << "[PTU::SetTimeouts] Setting timeouts...";
		serPort.setTimeouts(1000, 1, 1000, 1, 1000);
		cout << "OK" << endl;

		cout << "[PTU::setBaudRate] Setting baud rate...";
		serPort.setConfig(9600);
		cout << "OK" << endl;

		// PTU initial configuration
		cout << "[PTU::setInitialConfiguration] Setting initial "
				"configuration...";
		if ((!verbose(true)) ||	 // Original: false	Actual: true
			(!resolution()) || (!echoMode(true)) || (!inmediateExecution(true)))
		{
			cout << " Error setting initial configuration";
			serPort.close();
			return false;
		}

		cout << "OK" << endl
			 << endl
			 << "[INFO] Pan Resolution: " << panResolution << " radians | "
			 << RAD2DEG(panResolution) << "degrees";
		cout << endl
			 << "[INFO] TitlResolution: " << tiltResolution << " radians | "
			 << RAD2DEG(tiltResolution) << "degrees" << endl
			 << endl;
	}
	catch (exception& e)
	{
		cerr << e.what() << endl;
		return false;
	}

	return true;
}

/*-------------------------------------------------------------
							close
-------------------------------------------------------------*/

void CPtuDPerception::close() { serPort.close(); }
/*-------------------------------------------------------------
						  radError
-------------------------------------------------------------*/

double CPtuDPerception::radError(char axis, double nRadMoved)
{
	double div;

	if (axis == Pan)
		div = nRadMoved - long(nRadMoved / panResolution) * panResolution;
	else
		div = nRadMoved - long(nRadMoved / tiltResolution) * tiltResolution;

	return div;
}

/*-------------------------------------------------------------
						  transmit
-------------------------------------------------------------*/

bool CPtuDPerception::transmit(const char* command)
{
	char str[20] = "";

	strcpy(str, command);
	strcat(str, " ");

	size_t written = serPort.Write(str, strlen(str));

	if (!written) { return false; }

	return true;
}

/*-------------------------------------------------------------
						  receive
-------------------------------------------------------------*/

bool CPtuDPerception::receive(const char* command, char* response)
{
	int cnt = 0;
	unsigned long nReaden;
	char str[150] = "";
	char* tmp;

	do
	{
		nReaden = serPort.Read(&str[cnt], 1);
		if (nReaden != 0) cnt++;
	} while (
		(nReaden != 0) &&
		(((tmp = strstr(str, command)) == nullptr) || (str[cnt - 1] != '\n')));

	if (nReaden == 0)
	{
		nError = nError * TimeoutError;
		return false;
	}

	if (response != nullptr)
	{
		//*response=new char[150];
		strcpy(response, tmp);
	}

	// cout << str << endl;

	if (strstr(tmp, "!") == nullptr)
	{
		nError = nError * NoError;
		return true;
	}

	if ((strstr(tmp, "!P") != nullptr) && (strstr(tmp, "!T") != nullptr))
		nError = nError * PanTiltHitError;
	else if (strstr(tmp, "!T") != nullptr)
		nError = nError * TiltHitError;
	else if (strstr(tmp, "!P") != nullptr)
		nError = nError * PanHitError;
	else if (strstr(tmp, "! Maximum") != nullptr)
		nError = nError * MaxLimitError;
	else if (strstr(tmp, "! Minimum") != nullptr)
		nError = nError * MinLimitError;
	else if (strstr(tmp, "! Value") != nullptr)
		nError = nError * OutOfRange;
	else if (strstr(tmp, "! Illegal") != nullptr)
		nError = nError * IllegalCommandError;
	else
		nError = nError * UnExpectedError;

	return false;
}

/*-------------------------------------------------------------
						  verboseQ
-------------------------------------------------------------*/

bool CPtuDPerception::verboseQ(bool& mode)
{
	char response[150];

	if (!transmit("F") || !receive("F", response)) return false;

	if (strstr(response, "VERBOSE") != nullptr) mode = true;
	else
		mode = false;

	return true;
}

/*-------------------------------------------------------------
						  verbose
-------------------------------------------------------------*/

bool CPtuDPerception::verbose(bool set)
{
	if (set) return (transmit("FV") && (receive("FV", nullptr)));
	else
		return (transmit("FT") && (receive("FT", nullptr)));
}

/*-------------------------------------------------------------
						  echoModeQ
-------------------------------------------------------------*/

bool CPtuDPerception::echoModeQ(bool& mode)
{
	char response[150];

	if (!transmit("E") || !receive("E", response)) return false;

	if (strstr(upperCase(response).c_str(), "ENABLE") != nullptr) mode = true;
	else
		mode = false;

	return true;
}

/*-------------------------------------------------------------
						  echoMode
-------------------------------------------------------------*/

bool CPtuDPerception::echoMode(bool mode)
{
	if (mode) return (transmit("EE") && receive("EE", nullptr));
	else
		return (transmit("ED") && receive("ED", nullptr));
}

/*-------------------------------------------------------------
						  resolution
-------------------------------------------------------------*/

bool CPtuDPerception::resolution()
{
	char response[150];

	if ((!transmit("PR")) || (!receive("PR", response))) return false;
	panResolution = DEG2RAD(convertToDouble(response) / 3600);

	if ((!transmit("TR")) || (!receive("TR", response))) return false;
	tiltResolution = DEG2RAD(convertToDouble(response) / 3600);

	return true;
}

/*-------------------------------------------------------------
						 radQuerry
-------------------------------------------------------------*/

bool CPtuDPerception::radQuerry(char axis, char command, double& rad)
{
	char response[150];
	char sTrans[3];

	sTrans[0] = axis;
	sTrans[1] = command;
	sTrans[2] = '\0';

	if ((!transmit(sTrans)) || (!receive(sTrans, response))) return false;

	rad = posToRad(axis, convertToLong(response));

	return true;
}

/*-------------------------------------------------------------
						  radAsign
-------------------------------------------------------------*/

bool CPtuDPerception::radAsign(char axis, char command, double nRad)
{
	char sPos[20];
	char sTrans[22];

	char response[150];

	os::sprintf(sPos, sizeof(sPos), "%li", radToPos(axis, nRad));

	sTrans[0] = axis;
	sTrans[1] = command;
	strcpy(&sTrans[2], sPos);

	return (transmit(sTrans) && receive(sTrans, response));
}

/*-------------------------------------------------------------
						   scan
-------------------------------------------------------------*/

bool CPtuDPerception::scan(
	char axis, int tWait, float initial, float final, double radPre)
{
	// Check initial and final positions
	if (initial < final)
	{
		float aux = initial;
		initial = final;
		final = aux;
	}

	// Go to initial position
	moveToAbsPos(axis, initial);
	aWait();

	std::this_thread::sleep_for(500ms);

	double j = 0;
	offPosQ(axis, j);

	long steps = radToPos(axis, radPre);
	long totalSteps;

	// Obtain total number of steps
	int initialPos = radToPos(axis, initial);
	int finalPos = radToPos(axis, final);

	totalSteps = std::abs(initialPos - finalPos);

	// Performs first sweep
	for (int i = 0; i < totalSteps / steps; i++)
	{
		if (initial > final) { moveToOffPos(axis, -radPre); }
		else
		{
			moveToOffPos(axis, radPre);
		}
		offPosQ(axis, j);
		std::this_thread::sleep_for(std::chrono::milliseconds(tWait));
	}

	// Adjust steps for second scan
	moveToOffPos(axis, radPre / 2);
	aWait();

	// Performs seecond scan
	for (int i = 0; i < (totalSteps / steps) - 1; i++)
	{
		if (initial > final) { moveToOffPos(axis, radPre); }
		else
		{
			moveToOffPos(axis, -radPre);
		}
		offPosQ(axis, j);
		std::this_thread::sleep_for(std::chrono::milliseconds(tWait));
	}

	offPosQ(axis, j);

	// Return to initial position
	moveToAbsPos(axis, 0);

	return true;
}

/*-------------------------------------------------------------
						 radToPos
-------------------------------------------------------------*/

long CPtuDPerception::radToPos(char axis, double nrad)
{
	if (axis == Pan) return (long)(nrad / panResolution);
	else
		return (long)(nrad / tiltResolution);
}

/*-------------------------------------------------------------
						 posToRad
-------------------------------------------------------------*/

double CPtuDPerception::posToRad(char axis, long nPos)
{
	if (axis == Pan) return (double)nPos * panResolution;
	else
		return (double)nPos * tiltResolution;
}

/*-------------------------------------------------------------
						convertToLong
-------------------------------------------------------------*/

long CPtuDPerception::convertToLong(char* sLong)
{
	char* result = strpbrk(sLong, "-0123456789");
	char* stop;

	return strtol(result, &stop, 10);
}

/*-------------------------------------------------------------
					   convertToDouble
-------------------------------------------------------------*/

double CPtuDPerception::convertToDouble(char* sDouble)
{
	char* result = strpbrk(sDouble, "-0123456789");
	char* stop;

	return strtod(result, &stop);
}

/*-------------------------------------------------------------
					   checkError
-------------------------------------------------------------*/

int CPtuDPerception::checkErrors()
{
	int code = 0;

	// Check for errors
	if (noError()) { code = 0; }
	else
	{
		if (comError()) { code = 1; }
		else if (timeoutError())
		{
			code = 2;
		}
		else if (initError())
		{
			code = 3;
		}
		else if (panTiltHitError())
		{
			code = 4;
		}
		else if (panHitError())
		{
			code = 5;
		}
		else if (tiltHitError())
		{
			code = 6;
		}
		else if (maxLimitError())
		{
			code = 7;
		}
		else if (minLimitError())
		{
			code = 8;
		}
		else if (outOfRange())
		{
			code = 9;
		}
		else if (illegalCommandError())
		{
			code = 10;
		}
		else if (unExpectedError())
		{
			code = 11;
		}
	}

	return code;
}

/*-------------------------------------------------------------
					   nVersion
-------------------------------------------------------------*/

void CPtuDPerception::nversion(double& nVersion)
{
	cout << "[ERROR] Function not defined for this PTU model";
	nVersion = 0;
}

/*-------------------------------------------------------------
					   setLimits
-------------------------------------------------------------*/

bool CPtuDPerception::setLimits(
	[[maybe_unused]] char axis, [[maybe_unused]] double& l,
	[[maybe_unused]] double& u)
{
	cout << "[ERROR] Function not defined for this PTU model";
	return false;
}

/*-------------------------------------------------------------
					   changeMotionDir
-------------------------------------------------------------*/

bool CPtuDPerception::changeMotionDir()
{
	cout << "[ERROR] Function not defined for this PTU model";
	return false;
}

/*-------------------------------------------------------------
						rangeMeasure
-------------------------------------------------------------*/

bool CPtuDPerception::rangeMeasure()
{
	cout << "[ERROR] Function not defined for this PTU model";
	return false;
}
